%   opt2surf 
%   Christopher J. Whalen
%   University of Illinois
%   04/25/05 - Date Created
%   05/05/05 - Last Modified


%   Purpose - to move the digitized optode points onto the radially projected closest point on
%   the surface of the head.



function  [xyz] = opt2surf(xyz,xyz_mask) % [xyz] = opt2surf(xyz,xyz_mask)

% hold on;
% scatter3(xyz(:,1),xyz(:,2),xyz(:,3),'b');
% hold on;


% Trying out on Sphere to validate algorithm
% xyz_mask = zeros(1000,3);
% for i=1:1000
%     x = rand(1)*10;
%     y = rand(1)*(sqrt(10^2-x^2));
%     z = sqrt(10^2 - x^2 - y^2);
%     xyz_mask(i,1)=x;
%     xyz_mask(i,2)=y;
%     xyz_mask(i,3)=z;
% end
% xyz= zeros(100,3);
% for i=1:100
%     xyz(i,1) = rand(1)*20;
%     xyz(i,2) = rand(1)*20;
%     xyz(i,3) = rand(1)*20;
% end
% 
% xyz_pre = xyz;
% 
% scatter3(xyz_mask(:,1),xyz_mask(:,2),xyz_mask(:,3),'r');
% hold on
% scatter3(xyz(:,1),xyz(:,2),xyz(:,3),'b');


% fprintf('\n     opt2surf');
% fprintf('\n  Radial Projection\n');
% fprintf('Estimated Time: 1.0 min\n');
% tic


% Preallocating
xyz_sph = zeros(size(xyz,1),size(xyz,2)); % making Nx3 array
xyz_mask_sph = zeros(size(xyz_mask,1),size(xyz_mask,2));


% Converting into Spherical
[xyz_sph(:,1),xyz_sph(:,2),xyz_sph(:,3)] = cart2sph(xyz(:,1),xyz(:,2),xyz(:,3));
[xyz_mask_sph(:,1),xyz_mask_sph(:,2),xyz_mask_sph(:,3)] = cart2sph(xyz_mask(:,1),xyz_mask(:,2),xyz_mask(:,3));

% Searching for Closest Scalp Point
near_points = zeros(0,3);
for i=1:length(xyz_sph)
    % Parameters
	del_rho = 5; % 5 mm window to find rho
	del_theta = 0.05; % this is about 10 mm on the surface of the head
	del_phi = 0.05;
    %xyz_sph(i,:)
    while (size(near_points,1) == 0) % | (size(near_points,1)>=1000) % while loop makes sure near_points are not empty arrays
        near_pts_index = find( ((xyz_sph(i,1)- del_theta) < xyz_mask_sph(:,1)) & (xyz_mask_sph(:,1) < (xyz_sph(i,1) + del_theta)) & ((xyz_sph(i,2)- del_phi) < xyz_mask_sph(:,2)) & (xyz_mask_sph(:,2) < (xyz_sph(i,2) + del_phi)) & ((xyz_sph(i,3)- del_rho) < xyz_mask_sph(:,3)) & (xyz_mask_sph(:,3) < (xyz_sph(i,3) + del_rho)));
        %i
        %size(near_pts_index,1)
        near_points = zeros(size(near_pts_index,1),3);
        for np=1:length(near_pts_index) % This may be unnecessarily slow
            near_points(np,:) = xyz_mask_sph(near_pts_index(np),:);
        end
        if size(near_points,1)==0
            del_rho = 1.2*del_rho;
            del_theta = 2*del_theta;
            del_phi = 2*del_phi;
        end
%         if size(near_points,1)>=1000  % cutting down near_points
%             del_rho = del_rho/1.5
%             del_theta = del_theta/3
%             del_phi = del_phi/3
%         end
%         if (del_rho >= 40) | (del_theta >= 2*pi) | (del_phi >= 2*pi) % exit loop if angles complete circle.
%             near_points = zeros(1,3);
%             fprintf('\nAngles Exceed 2pi\n');
%         end
    end
    error_matrix = zeros(size(near_points,1),3);
    error_matrix = repmat(xyz_sph(i,:),size(near_points,1),1) - near_points; % the error btw elp and each near point in MRI
    dist_matrix = zeros(size(near_points,1),1); 
    for k=1:size(error_matrix,1)
        dist_matrix(k) = (error_matrix(k,1))^2 + (error_matrix(k,2))^2; % cost funtion is sum of squares of error in theta and phi
    end
    [min_dist,index] = min(dist_matrix); % finding the nearest radial point
    index = index;
    xyz_sph(i,:) = xyz_mask_sph(near_pts_index(index),:);
    near_points = zeros(0,3);
end


% Converting back into Cartesian
[xyz_new(:,1),xyz_new(:,2),xyz_new(:,3)] = sph2cart(xyz_sph(:,1),xyz_sph(:,2),xyz_sph(:,3));

xyz_diff = xyz_new - xyz;
for i=1:length(xyz)
    xyz_dist(i,1) = (xyz_diff(i,1)^2 + xyz_diff(i,2)^2 + xyz_diff(i,3)^2);
end
xyz_dist;

xyz = xyz_new;

% hold on;
% scatter3(xyz(:,1),xyz(:,2),xyz(:,3),'g','filled');
% hold on;

% hold on
% scatter3(xyz(:,1),xyz(:,2),xyz(:,3),'g','filled');
% for i=1:length(xyz)
%     plot3([xyz_pre(i,1) xyz(i,1)],[xyz_pre(i,2) xyz(i,2)],[xyz_pre(i,3) xyz(i,3)],'k')
% end

% fprintf('Actual Time: %2.1f min \n',toc/60);
% fprintf('     opt2surf done\n');
